
/*
*/


#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "rp2040.h"
#include "hardware/resets.h"
#include "hardware/pio.h"
#include "hardware/pio_instructions.h"

#include "umsg.h"
#include "dcchan.h"


/*

从 rp2040 角度看来, read 和 write 都是从输出若干 bit 开始的.
所以写入到 FIFO 

对于 read 操作, 只需要支持 32 位的 dr 寄存器.
因为从 had 模块的 verilog 代码看来, 对于 8 位的 ir 寄存器, read 操作仅仅返回全 1 ???

在 read 动作之前, 需要发送 start,rw,rs 总共 4 个 bits, 这个可以通过 write 操作完成.
如果在之前有 write ir 寄存器, 可以把这 4 个 bits 合并在一起完成.

read 的结果通过 FIFO - output 返回给用户程序, 一共是 2 个 word .
word-0 是读操作采集的 32 bits 寄存器内容.
word-1 只有最低 2 bits 有效, 
bit-0 是采集到的 odd parity, 可以用来判断是否正常.
bit-1 是对应时序中的 SYNC 周期的值, 按照协议应当是 低电平 0 .
这两个 bit 可以用于检查. 对应错误场景是 dut 断电? 线路短路等?

*/

#define had3_offset_read_stage 10u
#define had3_offset_finish_nty 25u

static const uint16_t had3_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block
    0x7201, //  1: out    pins, 1         side 0 [2]
    0x6025, //  2: out    x, 5
    0x6045, //  3: out    y, 5
    0x1929, //  4: jmp    !x, 9           side 1 [1]
    0x0046, //  5: jmp    x--, 6
    0xa142, //  6: nop                           [1]
    0x7401, //  7: out    pins, 1         side 0 [4]
    0x1a46, //  8: jmp    x--, 6          side 1 [2]
    0xa0a2, //  9: mov    pc, y
    
    0xa042, // 10: nop
    0xf480, // 11: set    pindirs, 0      side 0 [4]
    0xfc01, // 12: set    pins, 1         side 1 [4]
    0xf43f, // 13: set    x, 31           side 0 [4]
    0x5a01, // 14: in     pins, 1         side 1 [2]
    0xa056, // 15: mov    y, ::isr
    0xa042, // 16: nop
    0xb442, // 17: nop                    side 0 [4]
    0x5a01, // 18: in     pins, 1         side 1 [2]
    0x0050, // 19: jmp    x--, 16
    0x8020, // 20: push   block
    0xb442, // 21: nop                    side 0 [4]
    0x5c01, // 22: in     pins, 1         side 1 [4]
    0xf481, // 23: set    pindirs, 1      side 0 [4]
    0x5c5f, // 24: in     y, 31           side 1 [4]

    0x8020, // 25: push   block
    0xc000, // 26: irq    nowait 0
    0x0000, // 27: jmp    0
            //     .wrap
};


static const struct pio_program had3_program = {
    .instructions = had3_program_instructions,
    .length = 28,
    .origin = -1,
};


void PIO0_Q0_Handler( void )
{
    PIO pio = pio0;
    uint sm = 0;
    uint32_t tary[4];
    uint rcnt;

    /* clear */
    pio_interrupt_clear( pio, 0 );

    /* get result from fifo */
    rcnt = pio_sm_get_rx_fifo_level( pio, sm );
    if ( rcnt == 2 ) {
        
        tary[1] = 2;
        tary[2] = pio_sm_get( pio, sm );
        tary[3] = pio_sm_get( pio, sm );

    } else if ( rcnt == 1 ) {
        
        tary[1] = 1;
        tary[2] = pio_sm_get( pio, sm );
        tary[3] = 0;

    } else {
        /* error */
        tary[1] = 0xffffffff;
        tary[2] = 0;
        tary[3] = 0;
    }

    /* send to user */
    tary[0] = SYS_PIOS;
    sysmsg_send_to_user( 4, tary );
    return;
}


static int had_intf_init( uint tck, uint tms, uint32_t freq )
{
    PIO pio = pio0;
    uint sm = 0;
    uint clk_pin = tck;
    uint tms_pin = tms;
    pio_sm_config cfg;

    /* unreset */
    unreset_block_wait( RESETS_RESET_PIO0_BITS );

    /* gpio */
    gpio_pull_up( clk_pin );
    gpio_pull_up( tms_pin );
    pio_gpio_init( pio, clk_pin );
    pio_gpio_init( pio, tms_pin );
    pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true );
    pio_sm_set_consecutive_pindirs(pio, sm, tms_pin, 1, true );
    pio_sm_set_pins( pio, sm, (1 << clk_pin) | (1 <<tms_pin) );

    /* config */
    cfg = pio_get_default_sm_config();
    sm_config_set_out_pins( &cfg, tms_pin, 1 );
    sm_config_set_set_pins( &cfg, tms_pin, 1 );
    sm_config_set_in_pins( &cfg, tms_pin );
    sm_config_set_sideset_pins( &cfg, clk_pin );
    sm_config_set_sideset( &cfg, 2, true, false );
    sm_config_set_clkdiv_int_frac( &cfg, 12, 0 );

    /* INST_MEM */
    pio_add_program_at_offset( pio, &had3_program, 0 );

    /**/
    pio_sm_init( pio, sm, 0, &cfg );
    pio_sm_set_enabled( pio, sm, true );

    /* IRQ enable */
    pio_set_irq0_source_enabled( pio, pis_interrupt0, true );
    NVIC_EnableIRQ( PIO0_IRQ_0_IRQn );
    return 0;
}





/*
 cnt : 支持范围 1-22.
bits : 要输出的 bit 序列, 从最低 bit-0 开始串行输出.
 nxt : 执行之后跳转的 pc 值, 支持 3 个值, 0, 10, 25;
*/
static uint32_t had_encode_write( uint cnt, uint32_t bits, uint nxt )
{
    uint32_t t0;

    t0 = bits & 0x1;
    bits = bits >> 1;
    bits = (bits << 5) | nxt;
    bits = (bits << 5) | (cnt - 1);
    bits = (bits << 1) | t0;

    return bits;
}


static int had_enc_null_clock( uint32_t * pdat )
{
    pdat[0] = had_encode_write( 22, 0x3FFFFF, 0 );
    pdat[2] = pdat[1] = pdat[0];
    pdat[3] = had_encode_write( 22, 0x3FFFFF, had3_offset_finish_nty );    
    return 4;
}


static int had_enc_write_by_index( uint8_t idx, uint32_t vdr, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /**/
    pary = __builtin_popcount( idx ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | idx;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = had_encode_write( 15, bits, 0 );
    pdat[0] = bits;

    /**/
    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */
    bits = had_encode_write( 21, bits, 0 );
    pdat[1] = bits;

    /**/
    pary = __builtin_popcount( vdr ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 16) | (vdr >> 16);
    bits = had_encode_write( 18, bits, had3_offset_finish_nty );
    pdat[2] = bits;    
    
    /**/
    return 3;
}


static int had_enc_read_by_index( uint8_t idx, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = 0xE;
    pary = __builtin_popcount( idx ) + 1;
    bits = (bits << 2) | 0x2 | (pary & 0x1);
    bits = (bits << 8) | idx;

    /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = (bits << 5) | 0x18;
    bits = had_encode_write( 19, bits, had3_offset_read_stage );

    /**/
    *pdat = bits;
    return 1;
}


static int had_enc_write_dr32( uint32_t vdr, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);

    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */

    pdat[0] = had_encode_write( 21, bits, 0 );
    pdat[1] = had_encode_write( 18, pary, had3_offset_finish_nty );

    /**/
    return 2;
}


static int had_check_parity( uint32_t value, uint32_t parity )
{
    if ( ( (__builtin_popcount(value) + 1) & 0x01 ) != parity ) {
        return -1;
    } else {
        return 0;    
    }
}


int had_null_clock( void )
{
    PIO pio = pio0;
    int sm = 0; 
    uint32_t temp;
    uint32_t bit0;

    /**/
    temp = had_encode_write( 22, 0x3FFFFF, 0 );
    pio_sm_put_blocking( pio, sm, temp );
    
    temp = had_encode_write( 22, 0x3FFFFF, 0 );
    pio_sm_put_blocking( pio, sm, temp );

    temp = had_encode_write( 22, 0x3FFFFF, 0 );
    pio_sm_put_blocking( pio, sm, temp );

    temp = had_encode_write( 22, 0x3FFFFF, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, temp );

    /* wait finish */
    pio_sm_get_blocking( pio, sm );
    return 0;
}



int had_ir8_write( uint8_t vir )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vir ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */

    /**/
    bits = had_encode_write( 15, bits, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_write( uint32_t vdr )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);

    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */

    bits = had_encode_write( 21, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    pary = had_encode_write( 18, pary, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, pary );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_read( uint32_t *pvdr )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = had_encode_write( 4, 0xE, had3_offset_read_stage );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = pio_sm_get_blocking( pio, sm );
    pary = pio_sm_get_blocking( pio, sm );

    /* check parity */
    if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
        *pvdr = bits;
        return (int)(pary + 0x100);
    }

    /* ok, end */
    *pvdr = bits;
    return 0;
}


int had_dr32_write_by_index( uint8_t vir, uint32_t vdr )
{
    /**/
    PIO pio = pio0;
    int sm = 0;
    uint32_t bits;
    uint32_t pary;

    /**/
    pary = __builtin_popcount( vir ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = had_encode_write( 15, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */
    bits = had_encode_write( 21, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);
    pary = had_encode_write( 18, pary, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, pary );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_read_by_index( uint8_t vir, uint32_t *pvdr )
{
    /**/
    PIO pio = pio0;
    int sm = 0;
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = 0xE;
    pary = __builtin_popcount( vir ) + 1;
    bits = (bits << 2) | 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;

    /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = (bits << 5) | 0x18;          
    bits = had_encode_write( 19, bits, had3_offset_read_stage );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = pio_sm_get_blocking( pio, sm );
    pary = pio_sm_get_blocking( pio, sm );

    /* check parity */
    if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
        return (int)(pary + 0x100);
    }

    /* ok, end */
    *pvdr = bits;
    return 0;
}


/*
* 前提条件是处于 debug 模式.
* 使用 DDC 方式来写内存?
*/
int e902_write_memory( uint32_t addr, int tsiz, uint8_t * ptr )
{
    int i;
    uint32_t temp;

    /* DDCEN = 1, bit-20 */
    had_dr32_write_by_index( HREG_HCR, 0x100000 );
    had_dr32_write_by_index( HREG_ADDR, addr );
    had_ir8_write( HREG_DATA );

    /**/
    for ( i=0; i<tsiz; i+=4 ) {

        temp = ptr[i+3];
        temp = (temp << 8) + ptr[i+2];
        temp = (temp << 8) + ptr[i+1];
        temp = (temp << 8) + ptr[i];


        had_dr32_write( temp );
    }

    return 0;
}


static uint32_t inst_encode_csrr( uint idx, uint rd )
{
    uint32_t temp;

    /* CSRRS rd, csr, rs  : csr(12), rs1(5), func=3'b010, rd(5), opcode=7'b1110011 */
    temp = idx;
    temp = (temp << 8) | 0x2;
    temp = (temp << 5) | rd;
    temp = (temp << 7) | 0x73;

    /**/
    return temp;
}


static uint32_t inst_encode_csrw( uint idx, uint rs )
{
    uint32_t temp;

    /* CSRRW x0, csr, rs  : csr(12), rs1(5), func=3'b001, rd(5), opcode=7'b1110011 */
    temp = idx;
    temp = (temp << 5) | rs;
    temp = (temp << 15) | 0x1073;

    /**/
    return temp;
}


static uint32_t inst_encode_mov( uint rd, uint rs )
{
    uint32_t temp;

    /* ADDI rd, rs, 0 :  imm(12), rs1(5), func=3'b000, rd(5), opcode=7'b0010011 */
    temp = rs;
    temp = (temp << 8) | rd;
    temp = (temp << 7) | 0x13;

    /**/
    return temp;
}


/*
* 使用 IR 寄存器, 让 cpu 执行 CSRRS x1, csr, x0
* 使用 WBBR 获得指令执行的结果.
*/
int e902_read_csr( uint idx, uint32_t * pcsr )
{
    int iret;

    /* DDC=0, FFY=0 */
    had_dr32_write_by_index( HREG_HCR, 0x0 );
    had_dr32_write_by_index( HREG_CSR, 0x0 );

    /* GO=1 */
    had_dr32_write_by_index(  0x40 | HREG_IR, inst_encode_csrr( idx, 1 ) );
    // had_dr32_write_by_index(  0x40 | HREG_IR, inst_encode_mov( 1, 1 ) );

    /**/
    iret = had_dr32_read_by_index( HREG_WBBR, pcsr );
    had_ir8_write( HREG_BYPASS );
    return iret;
}


/*
* 使用 IR 寄存器, 让 cpu 执行 CSRRW x0, csr, x1
* 使用 WBBR 获得指令执行的结果.
*/
int e902_write_csr( uint idx, uint32_t csrv )
{
    int iret;

    /* DDC=0, FFY=0 */
    had_dr32_write_by_index( HREG_HCR, 0x0 );
    had_dr32_write_by_index( HREG_CSR, 0x100 );

    /* GO=1 */
    had_dr32_write_by_index( HREG_WBBR, csrv );
    had_dr32_write_by_index(  0x40 | HREG_IR, inst_encode_csrw( idx, 1 ) );

    /**/
    had_dr32_write_by_index( HREG_CSR, 0x0 );
    had_ir8_write( HREG_BYPASS );
    return iret;
}


/*
* 读出来指定数量的通用寄存器的值.
*/
int e902_read_gpr( uint idx, uint cnt, uint32_t * pgpr )
{
    int iret;
    uint i;

    /* DDC=0, FFY=0 */
    had_dr32_write_by_index( HREG_HCR, 0x0 );
    had_dr32_write_by_index( HREG_CSR, 0x0 );

    for ( i=0; i<cnt; i++ ) {
        
        /**/
        idx = idx & 0x1F;

        /* GO=1 */
        had_dr32_write_by_index(  0x40 | HREG_IR, inst_encode_mov( idx, idx ) );
        iret = had_dr32_read_by_index( HREG_WBBR, (pgpr + i) );
        if ( iret != 0 ) {
            break;
        }

        /**/
        idx += 1;
    }

    /**/
    had_ir8_write( HREG_BYPASS );
    return iret;

}


int e902_write_gpr( uint idx, uint cnt, uint32_t * pgpr )
{
    uint i;

    /* DDC=0, FFY=1 */
    had_dr32_write_by_index( HREG_HCR, 0x0 );
    had_dr32_write_by_index( HREG_CSR, 0x100 );

    for ( i=0; i<cnt; i++ ) {
        
        /**/
        idx = idx & 0x1F;

        /* GO=1 */
        had_dr32_write_by_index( HREG_WBBR, *(pgpr + i) );
        had_dr32_write_by_index(  0x40 | HREG_IR, inst_encode_mov( idx, idx ) );

        /**/
        idx += 1;        
    }

    /**/
    had_dr32_write_by_index( HREG_CSR, 0x0 );
    had_ir8_write( HREG_BYPASS );
    return 0;
}


/*
* 通过 DDERX 寄存器, 发送给 e902 上运行的软件.
* 每次写入一个 32 bits .
* 最高 8 bits 是个 flag, 表示后面 24 bit 的有效长度.
*
* 如果 e902 上长时间不接收, 要考虑超时判断.
* 双工如何实现?
*/
int had_umsg_send( umsg_t * pmsg )
{
    uint32_t temp;
    uint32_t tary[24];
    int tnum;
    int ttlen;
    int ttofs;
    int i;

    /**/
    pmsg->data[pmsg->mlen] = pmsg->type;
    pmsg->mlen += 1;

    /* encoding */
    if ( pmsg->mlen < 2 ) {
        umsg_free( pmsg );
        return 1;
    }

    if ( pmsg->mlen == 2 ) {
        
        temp = 0xA500;
        temp = (temp << 8) | pmsg->data[1];
        temp = (temp << 8) | pmsg->data[0];

        /**/
        tary[0] = temp;
        tnum = 1;

    } else if ( pmsg->mlen == 3 ) {
        
        temp = 0x5A;
        temp = (temp << 8) | pmsg->data[2];
        temp = (temp << 8) | pmsg->data[1];
        temp = (temp << 8) | pmsg->data[0];

        /**/
        tary[0] = temp;
        tnum = 1;

    } else {

        temp = 0x55;
        temp = (temp << 8) | pmsg->data[2];
        temp = (temp << 8) | pmsg->data[1];
        temp = (temp << 8) | pmsg->data[0];

        /**/
        tary[0] = temp;
        tnum = 1;
        ttlen = pmsg->mlen - 3;
        ttofs = 3;

        /* TAG middle */
        while ( ttlen > 3 ) {

            /**/
            temp = 0xAA;
            temp = (temp << 8) | pmsg->data[ttofs + 2];
            temp = (temp << 8) | pmsg->data[ttofs + 1];
            temp = (temp << 8) | pmsg->data[ttofs + 0];
            tary[tnum] = temp;
            tnum += 1;

            /**/
            ttlen -= 3;
            ttofs += 3;
        }

        /* TAG tail */
        switch ( ttlen ) {

        case 1:
            temp = 0xCA0000;
            temp = (temp << 8) | pmsg->data[ttofs + 0];
            break;

        case 2:
            temp = 0xC500;
            temp = (temp << 8) | pmsg->data[ttofs + 1];
            temp = (temp << 8) | pmsg->data[ttofs + 0];
            break;

        case 3:
            temp = 0xC6;
            temp = (temp << 8) | pmsg->data[ttofs + 2];
            temp = (temp << 8) | pmsg->data[ttofs + 1];
            temp = (temp << 8) | pmsg->data[ttofs + 0];
            break;
        }

        /**/
        tary[tnum] = temp;
        tnum += 1;
    }

    
    /* write to had channel */
    for ( i=0; i<tnum; i++ ) {

        do {
            
            /* check time out */

            /**/
            had_dr32_read_by_index( HREG_EHSR, &temp );

        } while( (temp & 0x2) == 0x2 );

        /**/
        had_dr32_write_by_index( HREG_DDER, tary[i] );
    }

    return 0;
}



/**/
static had_async_session_t * sess_phead = NULL;
static had_async_session_t * sess_ptail = NULL;


static void had_async_to_pio( int tnum, uint32_t * preq )
{
    PIO pio = pio0;
    uint smx = 0;
    
    /**/
    for (int i=0; i<tnum; i++ ) {
        pio_sm_put_blocking( pio, smx, preq[i] );
    }

    return;
}


int had_async_alloc_session( async_resp_cbkf cbkf, void * parg, int autofree, had_async_session_t ** ppsess )
{
    had_async_session_t * psess;

    /**/
    psess = (had_async_session_t *)malloc( sizeof(had_async_session_t) );
    if ( psess == NULL ) {
        return 1;
    }

    /**/
    psess->next = NULL;
    psess->autofree = autofree;
    psess->req_num = 0;
    psess->cbkf = cbkf;
    psess->parg = parg;
    psess->step = 0;

    /**/
    *ppsess = psess;
    return 0;
}


int had_async_post_requst( had_async_session_t * psess, int tnum, uint32_t * preq )
{
    /**/
    psess->req_num = tnum;
    for ( int i=0; i<tnum; i++ ) {
        psess->req_ary[i] = preq[i];
    }

    /* add to list tail */
    if ( sess_ptail != NULL ) {
        sess_ptail->next = psess;
        sess_ptail = psess;
        return 0;
    }
    
    /**/
    sess_phead = psess;
    sess_ptail = psess;

    /**/
    had_async_to_pio( tnum, preq );
    psess->stats = 1;
    return 0;
}


/*
用于在 回调函数中, 更新请求数据.
*/
int had_async_refresh_requst( had_async_session_t * psess, int tnum, uint32_t * preq )
{
    /**/
    psess->req_num = tnum;
    for ( int i=0; i<tnum; i++ ) {
        psess->req_ary[i] = preq[i];
    }

    /**/
    return 0;
}


int had_async_refresh_callbk( had_async_session_t * psess, async_resp_cbkf cbkf, void * parg )
{
    psess->cbkf = cbkf;
    psess->parg = parg;

    /**/
    return 0;
}


int had_async_proc_resp( int tnum, uint32_t * prsp )
{
    int iret;
    had_async_session_t * psess;

    /**/
    if ( sess_phead == NULL ) {
        /* error, nothing */
        return 1;
    }


    /*
     * 回调函数的返回值, 可以有三种意义: 
     * == 0 : 表示会话完成, 可以删除释放 session 对象.
     *  < 0 : 会话需要继续, 并且放在队列开头, 适用于紧急的会话过程.
     *  > 0 : 会话需要继续, 并且放在队列末尾, 适用于慢速或者轮询状态的会话过程.
     */
    psess = sess_phead;
    iret = psess->cbkf( psess, tnum, prsp );
    if ( iret == 0 ) {

        /* free session */
        sess_phead = psess->next;
        if ( psess->autofree ) {
            free( psess );
        }

        /* try next session */
        if ( sess_phead == NULL ) {
            sess_ptail = NULL;
            return 0;
        }

    } else if ( iret > 0 ) {

        /* recycle to tail */
        if ( sess_phead != sess_ptail ) {
            sess_phead = psess->next;
            
            psess->next = NULL;
            sess_ptail->next = psess;
            sess_ptail = psess;
        }

    } else {

        /* nothing, already at head */
    }

    /**/
    psess = sess_phead;
    had_async_to_pio( psess->req_num, psess->req_ary );
    psess->stats += 1;
    return 0;
}


static void had_util_umsg_iret( umsg_t * pmsg, uint32_t iret )
{
    umsg_reset_ofs( pmsg );
    umsg_add_i32( pmsg, iret );
    umsg_send( pmsg );
    return;
}


static int had_cbk_null_clock( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, 0 );
    umsg_send( pmsg );
    return 0;
}


void had_umsg_null_clock( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_null_clock, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        umsg_skip( pmsg, pmsg->mlen );
        umsg_send( pmsg );
        return;
    }

    /**/
    tary[0] = had_encode_write( 22, 0x3FFFFF, 0 );
    tary[2] = tary[1] = tary[0];
    tary[3] = had_encode_write( 22, 0x3FFFFF, had3_offset_finish_nty );

    /**/
    had_async_post_requst( psess, 4, tary );
    return;
}


/*
*/
static int had_cbk_dr32_write( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, 0 );
    umsg_send( pmsg );
    return 0;    
}


void had_umsg_dr32_write( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];
    uint8_t idx;
    uint32_t vdr;

    /**/
    idx = umsg_get_u8( pmsg );
    vdr = umsg_get_u32( pmsg );

    /**/
    iret = had_async_alloc_session( had_cbk_dr32_write, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        return;
    }

    /**/
    iret = had_enc_write_by_index( idx, vdr, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


/*
*/
static int had_cbk_dr32_read( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;    
    umsg_reset_ofs( pmsg );

    /* check parity, [0]=dr32, [1]=parity  */
    if ( ( (__builtin_popcount(prsp[0]) + 1) & 0x01 ) != prsp[1] ) {
        umsg_add_u32( pmsg, 1234 );
    } else {
        umsg_add_u32( pmsg, 0 );
        umsg_add_u32( pmsg, prsp[0] );
    }

    /**/
    umsg_add_u32( pmsg, 0xaabb );
    umsg_send( pmsg );
    return 0;
}

void had_umsg_dr32_read( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];
    uint8_t idx;
    
    /**/
    idx = umsg_get_u8( pmsg );

    /**/
    iret = had_async_alloc_session( had_cbk_dr32_read, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        return;
    }

    iret = had_enc_read_by_index( idx, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}




/*
0 : null clock
1 :  read ID, verify conn ok.
2 :  read HSR, check already debug mode??
3 : write HCR, sync debug request.
2 :  read HSR, check already debug mode??
*/

static int had_cbk_debug_enter( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;


    switch ( psess->step ) {
    case 0:
        iret = had_enc_read_by_index( HREG_ID, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 1:
        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* return */
            had_util_umsg_iret( pmsg, -1 );
            iret = 0;
            break;
        }

        /* todo : check fix id, config?? */
        if ( prsp[0] != 0x18005b31 ) {
            had_util_umsg_iret( pmsg, -2 );
            iret = 0;
            break;
        }

        /* next step, read HSR */
        iret = had_enc_read_by_index( HREG_HSR, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 2:
        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* return */
            had_util_umsg_iret( pmsg, -3 );
            iret = 0;
            break;
        }
        
        /* HSR : already debug mode ?? */
        if ( (prsp[0] & 0x3) == 0x2 ) {
            had_util_umsg_iret( pmsg, 0 );
            iret = 0;
            break;
        }

        /* DR = 1, bit-15  */
        iret = had_enc_write_by_index( HREG_HCR, 0x8000, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 3:
        iret = had_enc_read_by_index( HREG_HSR, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 4:
        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* return */
            had_util_umsg_iret( pmsg, -4 );
            iret = 0;
            break;
        }
        
        /* HSR : already debug mode ?? */
        if ( (prsp[0] & 0x3) == 0x2 ) {
            had_util_umsg_iret( pmsg, 0 );
        } else {
            had_util_umsg_iret( pmsg, -5 );
        }

        /**/
        iret = 0;
        break;

    default:
        had_util_umsg_iret( pmsg, -100 );
        iret = 0;
        break;
    }

    /**/
    return iret;
}


void had_umsg_debug_enter( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];

    /**/
    iret = had_async_alloc_session( had_cbk_debug_enter, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        return;
    }

    /* send first request */
    iret = had_enc_null_clock( tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


/*
    // EX=1, GO=1
    had_dr32_write_by_index( HREG_PC, epc );
    had_dr32_write_by_index( HREG_HCR, 0 );
    had_dr32_write_by_index( 0x60 | HREG_BYPASS, 0 );

    // check
    iret = had_dr32_read_by_index( HREG_HSR, &temp );
    if ( iret != 0 ) {
        return 1;
    }
*/
static int had_cbk_debug_leave( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    switch ( psess->step ) {
    case 0:
        iret = had_enc_write_by_index( HREG_HCR, 0, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 1:
        iret = had_enc_write_by_index( 0x60 | HREG_BYPASS, 0, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    default:
        had_util_umsg_iret( pmsg, 0 );
        iret = 0;
        break;
    }

    /**/
    return iret;
}


void had_umsg_debug_leave( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];
    uint32_t temp;

    /**/
    temp = umsg_get_u32( pmsg );

    /**/
    iret = had_async_alloc_session( had_cbk_debug_leave, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        return;
    }

    /* send first request */
    iret = had_enc_write_by_index( HREG_PC, temp, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


/*
必须先进入 debug 模式, 因为 HAD 模块内部是使用 IR 执行指令方式来实现的.
只能支持 4 字节 word 方式写入.
 ofs = 0 : 4 字节的 address 字段部分.
 ofs = 4 : 若干 4 字节的 word, 根据长度字段判断数量.
*/
static int had_cbk_ddc_write( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;
    uint32_t temp;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    switch ( psess->step ) {
    case 0:
        /* write HREG_ADDR : */
        temp = umsg_get_u32( pmsg );
        iret = had_enc_write_by_index( HREG_ADDR, temp, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 1:
        /* write HREG_DATA : */
        temp = umsg_get_u32( pmsg );
        iret = had_enc_write_by_index( HREG_DATA, temp, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    default:
        if ( umsg_remains(pmsg) < 4 ) {
            had_util_umsg_iret( pmsg, 0 );
            iret = 0;
            break;
        }

        /* write DR32 only, NOT pre write IR8 */
        temp = umsg_get_u32( pmsg );
        iret = had_enc_write_dr32( temp, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;
    }

    /**/
    return iret;
}


void had_umsg_ddc_write( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];

    /* check umesg length */
    if ( umsg_remains(pmsg) < 8 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_async_alloc_session( had_cbk_ddc_write, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        return;
    }

    /* write HCR : DDCEN = 1, bit-20 */
    iret = had_enc_write_by_index( HREG_HCR, 0x100000, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_e902_csrr( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;
    uint idx;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    switch ( psess->step ) {
    case 0:
        /**/
        idx = umsg_get_u16( pmsg );
        idx = idx & 0xFFF;
        
        /* GO=1, IR : csrr rd, csr */
        iret = had_enc_write_by_index( 0x40 | HREG_IR, inst_encode_csrr( idx, 1 ), tary );
        had_async_refresh_requst( psess, iret, tary );
        
        psess->step += 1;
        iret = -1;
        break;
        
    case 1:
        /* read WBBR */
        iret = had_enc_read_by_index( HREG_WBBR, tary );
        had_async_refresh_requst( psess, iret, tary );

        psess->step += 1;
        iret = -1;
        break;

    case 2:
        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* return */
            had_util_umsg_iret( pmsg, -1 );
        } else {
            umsg_reset_ofs( pmsg );
            umsg_add_i32( pmsg, 0 );
            umsg_add_u32( pmsg, prsp[0] );
            umsg_send( pmsg );
        }

        iret = 0;
        break;
    }

    /**/
    return iret;
}


/*
req msg : csr(u16)
rsp msg : iret(i32), value(u32)
*/
void had_umsg_e902_csrr( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];

    /* check umesg length */
    if ( umsg_remains(pmsg) < 2 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_async_alloc_session( had_cbk_e902_csrr, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* CSR = 0 ( FFY = 0 )  */
    iret = had_enc_write_by_index( HREG_CSR, 0x0, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_e902_csrw( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;
    uint idx;
    uint32_t temp;

    /**/
    pmsg = (umsg_t *)psess->parg;

    switch ( psess->step ) {
    
    case 0:
        /* write WBBR */
        umsg_skip( pmsg, 2 );
        temp = umsg_get_u32( pmsg );
        iret = had_enc_write_by_index( HREG_WBBR, temp, tary );
        had_async_refresh_requst( psess, iret, tary );

        psess->step += 1;
        iret = -1;
        break;

    case 1:
        /* GO=1, IR : csrr rd, csr */
        pmsg->offs = pmsg->offs - 6;
        idx = umsg_get_u16( pmsg );
        iret = had_enc_write_by_index( 0x40 | HREG_IR, inst_encode_csrw( idx, 1 ), tary );
        had_async_refresh_requst( psess, iret, tary );

        psess->step += 1;
        iret = -1;
        break;

    case 2:
        iret = had_enc_write_by_index( HREG_HCR, 0, tary );
        had_async_refresh_requst( psess, iret, tary );

        psess->step += 1;
        iret = -1;
        break;

    case 3:
        /**/
        had_util_umsg_iret( pmsg, 0 );
        iret = 0;
        break;
    }

    return iret;
}


/*
req msg : csr(u16), value(u32)
rsp msg : iret(i32)
*/
void had_umsg_e902_csrw( umsg_t * pmsg )
{
    int iret;
    had_async_session_t * psess;
    uint32_t tary[4];

    /* check umesg length */
    if ( umsg_remains(pmsg) < 6 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_async_alloc_session( had_cbk_e902_csrw, (void *)pmsg, true, &psess );
    if ( iret != 0 ) {
        /* todo : alloc fail, report error msg */
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* CSR = 0x100 (FFY = 1) */
    iret = had_enc_write_by_index( HREG_CSR, 0x100, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}





#define TAG_ALL1        0xA6
#define TAG_ALL2        0xA5
#define TAG_ALL3        0xAA

#define TAG_HEAD        0x55
#define TAG_BODY        0x5A

#define TAG_TAIL1       0xCA
#define TAG_TAIL2       0xC5
#define TAG_TAIL3       0xC6



static int had_channel_enc_msg( umsg_t * pmsg, uint32_t * pdat )
{
    uint32_t temp;
    uint32_t tary[24];
    int tnum;
    int tlen;
    int tofs;

    /**/
    tlen = pmsg->mlen;

    if ( tlen == 1 ) {
        
        temp = TAG_ALL1;
        temp = (temp << 24) | pmsg->data[0];

        /**/
        pdat[0] = temp;
        return 1;
    }


    if ( tlen == 2 ) {
        
        temp = TAG_ALL2;
        temp = (temp << 16) | pmsg->data[1];
        temp = (temp << 8) | pmsg->data[0];

        /**/
        pdat[0] = temp;
        return 1;
    }

    if ( tlen == 3 ) {
        
        temp = TAG_ALL3;
        temp = (temp << 8) | pmsg->data[2];
        temp = (temp << 8) | pmsg->data[1];
        temp = (temp << 8) | pmsg->data[0];

        /**/
        pdat[0] = temp;
        return 1;
    }

    /* long message, TAG head.. */
    temp = TAG_HEAD;
    temp = (temp << 8) | pmsg->data[2];
    temp = (temp << 8) | pmsg->data[1];
    temp = (temp << 8) | pmsg->data[0];
    pdat[0] = temp;
    tnum = 1;
    tofs = 3;

    /* TAG middle */
    while ( (tlen - tofs) > 3 ) {

        /**/
        temp = TAG_BODY;
        temp = (temp << 8) | pmsg->data[tofs + 2];
        temp = (temp << 8) | pmsg->data[tofs + 1];
        temp = (temp << 8) | pmsg->data[tofs + 0];

        pdat[tnum] = temp;
        tnum += 1;
        tofs += 3;
    }

    /* TAG tail */
    switch ( tlen - tofs ) {

    case 1:
        temp = TAG_TAIL1;
        temp = (temp << 24) | pmsg->data[tofs + 0];
        break;

    case 2:
        temp = TAG_TAIL2;
        temp = (temp << 16) | pmsg->data[tofs + 1];
        temp = (temp << 8) | pmsg->data[tofs + 0];
        break;

    case 3:
        temp = TAG_TAIL3;
        temp = (temp << 8) | pmsg->data[tofs + 2];
        temp = (temp << 8) | pmsg->data[tofs + 1];
        temp = (temp << 8) | pmsg->data[tofs + 0];
        break;
    }

    /**/
    pdat[tnum] = temp;
    tnum += 1;
    return tnum;
}



typedef struct _tag_had_channel {
    
    /* status, action : */
    int action;

    /* parity error, max = 4 */
    uint32_t errpcnt;

    /* recv from cpu, msg */
    umsg_t * recv_msg;

    /* trans to cpu, msg */
    umsg_t * trans_msg;

    int tx_maxs;
    int tx_offs;
    uint32_t tx_wrds[24];

} had_channel_ctx_t;


/*
轮询 EHSR 的目的, 就是判断是否有从 CPU 上传的数据.
如果有数据, 这个就是处理函数, 在这里会把零散的 word 碎片, 重组为 umsg_t 结构.
返回值不重要...
*/
int had_channel_input_rx( had_channel_ctx_t * pctx, uint32_t value )
{
    uint8_t tag;
    umsg_t * pmsg;
    uint8_t * ptr;

    if ( pctx->recv_msg == NULL ) {
        
        pctx->recv_msg = umsg_alloc();
        if ( pctx->recv_msg == NULL ) {
            return 1;
        }

        /**/
        pctx->recv_msg->offs = 0;
    }

    /**/
    tag = (uint8_t)(value >> 24);
    value = value & 0xFFFFFF;
    pmsg = pctx->recv_msg;
    ptr = pmsg->data;

    switch (tag) {

    case TAG_ALL1:
        pmsg->offs = 0;
        umsg_add_u8( pmsg, (uint8_t)value );
        break;

    case TAG_ALL2:
        pmsg->offs = 0;
        umsg_add_u16( pmsg, (uint16_t)value );
        break;

    case TAG_ALL3:
        pmsg->offs = 0;
        umsg_add_u24( pmsg, value );
        break;

    case TAG_TAIL1:
        umsg_add_u8( pmsg, (uint8_t)value );
        break;

    case TAG_TAIL2:
        umsg_add_u16( pmsg, (uint16_t)value );
        break;
    
    case TAG_TAIL3:
        umsg_add_u24( pmsg, value );
        break;

    case TAG_HEAD:
        pmsg->offs = 0;
        /* fall through */

    case TAG_BODY:
        umsg_add_u24( pmsg, value );
        return 0;

    default:
        return 0;
    }

    /* finish, send to host */
    umsg_fwd_send( pmsg );
    pctx->recv_msg = NULL;
    return 0;
}


/*
查询下一个需要发送的 word.
如果没有需要发送的数据, iret = -1..
*/
int had_channel_next_tx( had_channel_ctx_t * pctx, uint32_t * pdat )
{

    if ( pctx->tx_maxs == 0 ) {

        if ( pctx->trans_msg == NULL ) {
            
            /* nothing to send */
            return -1;
        }
        
        /**/
        pctx->tx_maxs = had_channel_enc_msg( pctx->trans_msg, pctx->tx_wrds );
        pctx->tx_offs = 0;
        umsg_free( pctx->trans_msg );
        pctx->trans_msg = NULL;
        
    }

    /**/
    *pdat = pctx->tx_wrds[pctx->tx_offs];
    pctx->tx_offs += 1;

    if ( pctx->tx_offs >= pctx->tx_maxs ) {
        pctx->tx_maxs = 0;
        pctx->tx_offs = 0;
    }

    return 0;
}



/*
持续轮询 EHSR 寄存器, 判断其中的 TX/RX 通道满空状态.
根据通道状态, 决定是否 发送 或者 接收消息.
*/
static had_async_session_t * chan_psess;



static int had_cbk_channel_check( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    had_channel_ctx_t * pctx;
    uint32_t tary[4];
    uint32_t temp;

    /**/
    pctx = (had_channel_ctx_t *)chan_psess->parg;

    switch ( psess->step ) {

    case 0:

        /* 
            轮询请求开始后, action = 1, 表达状态, 轮询动作正在运转中.
            如果收到用户发来的 stop 消息, 并且检查到 action == 1,  才会设置 action = 0x55AA.
            在这个位置, 如果检查到 action == 0x55AA, 会退出轮询模式, 并且设置 action = 0xffff.
            由上看来: 这个 action 同时有两种意义: 表达了当前状态, 也表达了用户请求的动作.
         */
        if ( pctx->action == 0x55AA ) {
            pctx->action = 0xFFFF;
            iret = 0;
            break;
        }

        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* error?? cnt++ to threshold?? auto stop.. */
            pctx->errpcnt += 1;
            return 0;
        }
        
        if ( (prsp[0] & 0x4) == 0x4 ) {

            /* HAD << CPU, 有数据可以读取?? */
            iret = had_enc_read_by_index( HREG_DDER, tary );
            had_async_refresh_requst( psess, iret, tary );
            psess->step = 1;
            iret = 1;
            break;
        }

        if ( (prsp[0] & 0x2) == 0 ) {
            
            /* 
            HAD >> CPU, 有空间可以发送?? 
            相对来说, 发送的优先级要低一些.
            虽然 DDRRX 寄存器是两份, 可以实现双工通信.
            但是 HAD 串行访问模块是单工的, 某个时刻 或者读, 或者写.
            */
            iret = had_channel_next_tx( pctx, &temp );
            if ( iret == 0 ) {

                iret = had_enc_write_by_index( HREG_DDER, temp, tary );
                had_async_refresh_requst( psess, iret, tary );
                psess->step = 2;
                iret = 1;
                break;
            }

        }

        /* nothing to trans, continue to read EHSR */
        psess->step = 0;
        iret = 1;
        break;

    case 1:

        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {
            /* error?? cnt++ to threshold?? auto stop.. */
            pctx->errpcnt += 1;
            return 0;
        }

        /**/
        had_channel_input_rx( pctx, prsp[0] );

        /**/
        iret = had_enc_read_by_index( HREG_EHSR, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step = 0;
        iret = 1;
        break;

    case 2:

        /* nothing to check */
        /* continue to read EHSR */
        iret = had_enc_read_by_index( HREG_EHSR, tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step = 0;
        iret = 1;
        break;

    default:
        /* error */
        iret = 0;
        break;
    }

    /* continue, Add to tail */
    return iret;
}


void had_umsg_channel_start( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_channel_ctx_t * pctx;

    /**/
    pctx = (had_channel_ctx_t *)chan_psess->parg;
    
    pctx->action = 1;
    pctx->errpcnt = 0;
    pctx->recv_msg = NULL;
    pctx->trans_msg = NULL;
    pctx->tx_maxs = 0;
    pctx->tx_offs = 0;

    /**/
    chan_psess->step = 0;
    iret = had_enc_read_by_index( HREG_EHSR, tary );
    had_async_post_requst( chan_psess, iret, tary );

    /**/
    had_util_umsg_iret( pmsg, 0 );
    return;
}


void had_umsg_channel_stop( umsg_t * pmsg )
{
    had_channel_ctx_t * pctx;

    /**/
    pctx = (had_channel_ctx_t *)chan_psess->parg;

    /*  */
    if ( pctx->action == 1 ) {
        pctx->action = 0x55AA;
    }

    had_util_umsg_iret( pmsg, 0 );
    return;
}


/**/
int had_channel_forward_to_cpu( umsg_t * pmsg )
{
    had_channel_ctx_t * pctx;

    /**/
    pctx = (had_channel_ctx_t *)chan_psess->parg;
    if ( pctx->action == 1 ) {
        
        /* todo : queue msg.. */
        pctx->trans_msg = pmsg;

    } else {

        /**/
        umsg_free( pmsg );
        printf( "loop not working...\n" );
    }

    return 0;
}


int had_mod_init(  uint tck, uint tms, uint32_t freq )
{
    int iret;
    had_channel_ctx_t * pctx;

    /**/
    pctx = (had_channel_ctx_t *)malloc( sizeof(had_channel_ctx_t) );
    if ( pctx == NULL ) {
        return 1;
    }

    pctx->action = 0;
    pctx->errpcnt = 0;
    pctx->recv_msg = NULL;
    pctx->trans_msg = NULL;
    pctx->tx_maxs = 0;
    pctx->tx_offs = 0;

    /**/
    iret = had_async_alloc_session( had_cbk_channel_check, (void *)pctx, false, &chan_psess );

    /**/
    iret = had_intf_init( tck, tms, freq );
    return iret;
}

extern uint32_t msg_pool_stats;

int had_debug_msg( umsg_t * pmsg )
{
    had_channel_ctx_t * pctx;

    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, (uint32_t)(uintptr_t)chan_psess );
    umsg_add_u32( pmsg, msg_pool_stats );
    umsg_add_u32( pmsg, 0 );

    umsg_send( pmsg );
    return 0;
}

